#region File information

/**
 * File: ConvexHull.cs
 * Date: 29-03-2008
 * Auth: Gerard v/d Lei
 * 
 * Desc: Described a ConvexHull from which an collision object can be build
 *       A convex hull is a collection of HullSide
 *       Methods are designed to be called from CollisionHull
 */

#endregion

using System.Collections.Generic;
using Microsoft.Xna.Framework;
using System;

namespace Gremlin.Collision
{
    /// <summary>
    /// A ConvexHull describes part of the CollisionHull. It's one convex geometric object, used for collision
    /// detection and responce.
    /// </summary>
    class ConvexHull
    {
        #region Attributes
        private HullSide[] _hullSides;
        private List<Vector3> _vertices;
        private string _name;
        private BoundingBox _boundingBox;
        private Vector3 _centerOfGravity;
        private float _radius;
        #endregion

        #region Constructors
        /// <summary>
        /// Constructs a ConvexHull from primitive data
        /// </summary>
        /// <param name="convexHull">Primitive data to contruct hull from</param>
        public ConvexHull(Dictionary<string, object> convexHull)
        {
            //Reads vertices and name from primitive data
            _vertices = (List<Vector3>)convexHull["vertices"];
            _name = (string)convexHull["name"];

            //Construct separate hulls
            int count = 0;
            List<Dictionary<string, object>> hullSides = (List<Dictionary<string, object>>)convexHull["hullside"];
            _hullSides = new HullSide[hullSides.Count];
            foreach (Dictionary<string, object> side in hullSides)
                _hullSides[count++] = new HullSide(side, ref _vertices);

            //Construct AABB, and a simple bounding sphere
            Vector3 min = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue), max = new Vector3(float.MinValue, float.MinValue, float.MinValue);
            foreach (Vector3 vertex in _vertices)
            {
                _centerOfGravity += vertex;
                min.X = Math.Min(min.X, vertex.X);
                min.Y = Math.Min(min.Y, vertex.Y);
                min.Z = Math.Min(min.Z, vertex.Z);
                max.X = Math.Max(max.X, vertex.X);
                max.Y = Math.Max(max.Y, vertex.Y);
                max.Z = Math.Max(max.Z, vertex.Z);
            }
            //Create bounding box
            _boundingBox = new BoundingBox(min, max);
            //Center of gravity together with radius gives a simple bounding sphere
            _centerOfGravity /= _vertices.Count;
            _radius = (float)Math.Sqrt(Math.Max((_centerOfGravity - min).LengthSquared(), (_centerOfGravity - max).LengthSquared()));
        }

        /// <summary>
        /// Makes a translated copy of a ConvexHull
        /// </summary>
        /// <param name="reference">Original to make copy from</param>
        /// <param name="transform">Transform matrix</param>
        /// <param name="rotation">Amount to rotate the normals</param>
        public ConvexHull(ConvexHull reference, Matrix transform, Quaternion rotation)
        {
            //Transform and copy all vertices
            _vertices = new List<Vector3>();
            Vector3 min = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue), max = new Vector3(float.MinValue, float.MinValue, float.MinValue);
            foreach (Vector3 vertex in reference._vertices)
            {
                //Transform vertices and add them to list
                Vector3 globalVertex = Vector3.Transform(vertex, transform);
                _vertices.Add(globalVertex);
                //Construct AABB and find new center of gravity
                _centerOfGravity += globalVertex;
                min.X = Math.Min(min.X, globalVertex.X);
                min.Y = Math.Min(min.Y, globalVertex.Y);
                min.Z = Math.Min(min.Z, globalVertex.Z);
                max.X = Math.Max(max.X, globalVertex.X);
                max.Y = Math.Max(max.Y, globalVertex.Y);
                max.Z = Math.Max(max.Z, globalVertex.Z);
            }
            //Create simple bounding sphere
            _centerOfGravity /= _vertices.Count;
            _radius = (float)Math.Sqrt(Math.Max((_centerOfGravity - min).LengthSquared(), (_centerOfGravity - max).LengthSquared()));
            //Create bounding box
            _boundingBox = new BoundingBox(min, max);
            //Copy name
            _name = reference._name;
            //Make new sides
            int sideCount = reference._hullSides.Length;
            _hullSides = new HullSide[sideCount];
            for (int cSide = 0; cSide < sideCount; cSide++)
            {
                //rotate normal
                Vector3 rNormal = reference._hullSides[cSide].Normal;
                rNormal = Vector3.Transform(rNormal, rotation);
                //calculate d
                float d = -Vector3.Dot(rNormal, _vertices[reference._hullSides[cSide].Indices[0]]);
                _hullSides[cSide] = new HullSide(ref reference._hullSides[cSide], rNormal, d, ref _vertices);
            }
        }
        #endregion

        #region Accessors

        /// <summary>
        /// Returns radius which is based on bounding box and center of gravity
        /// </summary>
        public float Radius
        {
            get{return _radius;}
        }

        /// <summary>
        /// Gets the number of vertices of this convex hull
        /// </summary>
        public int VertexCount
        {
            get { return _vertices.Count; }
        }

        /// <summary>
        /// Gets the center of gravity
        /// </summary>
        public Vector3 CenterOfGravity
        {
            get { return _centerOfGravity; }
        }

        /// <summary>
        /// Get bounding box
        /// </summary>
        public BoundingBox BoundingBox
        {
            get { return _boundingBox; }
        }

        #endregion

        /// <summary>
        /// Finds the point a ray intersects with this convex hull
        /// </summary>
        /// <param name="origin">Origin of ray</param>
        /// <param name="delta">Direction of ray</param>
        /// <param name="point">Point where ray intersects with hull</param>
        /// <returns>True when an intersection was found, false otherwise</returns>
        public bool RayIntersection(Vector3 origin, Vector3 delta, out Vector3 point)
        {
            //Quick check
            if(BoundingBox.Intersects(new Ray(origin, delta))==null)
            {
                point = new Vector3(0,0,0);
                return false;
            }
            //Check each side
            foreach (HullSide side in _hullSides)
            {
                //Ray can only intersect one side of a convex object
                if (side.RayIntersection(origin, delta, out point))
                    return true;
            }
            //Nothing found
            point = new Vector3();
            return false;
        }

        /// <summary>
        /// Finds the points a ray intersects with this convex hull, also checks
        /// if this happens on an edge.
        /// </summary>
        /// <param name="origin">Origin of ray</param>
        /// <param name="delta">Direction of ray</param>
        /// <param name="point">Point where ray intersects with hull</param>
        /// <param name="plane">The plane to which the ray intersected</param>
        /// <returns>True when an intersection was found, false otherwise</returns>
        public bool RayIntersection(Vector3 origin, Vector3 delta, out Vector3 point, out TPlane plane)
        {
            point = new Vector3();
            plane = new TPlane();
            //First to quick check against bounding box
            if (BoundingBox.Intersects(new Ray(origin, delta))==null)
                return false;
            //Check each side
            bool onEdge;
            Vector3 normal = new Vector3();
            Vector3 avgPoint = new Vector3();
            float d = 0;
            point = new Vector3();
            int foundIntersect = 0;
            //Check all edges
            foreach (HullSide side in _hullSides)
            {
                //Check for intersection with side, when onEdge, keep looking and average normal
                //If not on edge just return first hit
                if (side.RayIntersectionClamp(origin, delta, out point, out onEdge))
                {
                    if (!onEdge)
                    {
                        plane = new TPlane(side.Normal, side.D);
                        return true;
                    }else
                    {
                        foundIntersect++;
                        normal += side.Normal;
                        d += side.D; //All d's should be equal, but average for good measure
                        avgPoint += point;
                    }
                }
            }
            if (foundIntersect == 0)
                return false;
            //Average point and construct new plane of intersection
            normal.Normalize();
            plane = new TPlane(normal, d / foundIntersect);
            point = avgPoint / foundIntersect;

            return true;
        }

        /// <summary>
        /// Calculates how a plane divides points of this ConvexHull
        /// </summary>
        /// <param name="normal">Normal of the plane</param>
        /// <param name="d">D of the plane</param>
        /// <returns>0 if plane divides the points. 1 if all points before plane. -1 if all points beyond</returns>
        private int CalculatePlaneDivide(Vector3 normal, float d)
        {
            bool beyond = false;
            bool before = false;
            foreach (Vector3 point in _vertices)
            {
                //Check plane distance, return a signed distance, from this find how points
                //are divided by this plane
                float result = CollisionMath.DistancePointToPlane(normal, d, point);
                if(result < 0)
                    before = true; //There is atleast one point before
                else
                    beyond = true; //THere is atleast one point beyond
                if(beyond && before) //Plane divides
                    return 0;
            }
            //Plane doesn't divide, so all point lie either before or beyond
            if(beyond)
                return -1;
            else
                return 1;
        }

        /// <summary>
        /// Calculates the intersection between two convexhulls
        /// </summary>
        /// <param name="convexHull">ConvexHull to check against</param>
        /// <param name="points">Points of intersection</param>
        /// <returns>True when an intersection is found, false otherwise</returns>
        public bool ConvexHullIntersection(ConvexHull convexHull, out List<Vector3> points)
        {
            List<HullSide> sides0 = new List<HullSide>();
            List<HullSide> sides1 = new List<HullSide>();
            int divide = 0;
            //Check how sides in convexHull divide vertices of this
            foreach (HullSide side0 in convexHull._hullSides)
            {
                divide = CalculatePlaneDivide(side0.Normal, side0.D);
                if (divide == -1) //There can be no intersection
                {
                    points = new List<Vector3>();
                    return false;
                }
                if (divide == 0) //Save side to check later
                    sides0.Add(side0);
            }
            //Check how sides in this divide vertices in convexHull
            foreach (HullSide side1 in _hullSides)
            {
                divide = convexHull.CalculatePlaneDivide(side1.Normal, side1.D);
                if (divide == -1) //There can be no intersection
                {
                    points = new List<Vector3>();
                    return false;
                }
                if (divide == 0) //Save side to check later
                    sides1.Add(side1);
            }

            //O my, a collision
            Vector3[] point;
            points = new List<Vector3>();
            //Intersect saved sides
            foreach(HullSide side0 in sides0)
                foreach (HullSide side1 in sides1)
                {
                    //Find intersection between colliding hulls and save them
                    if (side0.HullSideIntersection(side1, out point))
                    {
                        points.Add(point[0]);
                        points.Add(point[1]);
                    }
                }
            //Highly unlikely, no collision found, means float point inprecision
            if (points.Count == 0)
            {
                points = new List<Vector3>();
                return false;
            }
            return true;
        }
    }
}
